#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include "sensor_msgs/msg/joint_state.hpp" 
#include <string>

unsigned char testSend3=0x07;
float pos[5] = {0};
int joint[5] = {0}; 
 
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};

union sendData
{
    short d;
    unsigned char data[2];
};
union sendData angle[7];

union receiveData
{
    short d;
    unsigned char data[2];
} leftVelNow, rightVelNow, angleNow;

class Serial_Node : public rclcpp::Node
{
public:
  Serial_Node() : Node("serial_node_cpp")
  {
    subscription_ = this->create_subscription<sensor_msgs::msg::JointState>(
        "/joint_states", 10, std::bind(&Serial_Node::Callback, this, std::placeholders::_1));

    // 等设备准备好再初始化
    // std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
    // 串口设备名（根据实际设备调整）
    const std::string device_name = "/dev/ttyACM0";
 
    RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
    // 创建串口配置对象
    // 波特率115200；不开启流控制；无奇偶效验；停止位1。
    drivers::serial_driver::SerialPortConfig config(
        115200,
        drivers::serial_driver::FlowControl::NONE,
        drivers::serial_driver::Parity::NONE,
        drivers::serial_driver::StopBits::ONE);
    
    // 初始化串口
    try
    {
      io_context_ = std::make_shared<drivers::common::IoContext>(1);
      // 初始化 serial_driver_
      serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
      serial_driver_->init_port(device_name, config);
      serial_driver_->port()->open();
      
      RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
      RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
      RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
    }
    catch (const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
      return;
    }

    // writeSpeed(joint, testSend3);
    // async_receive_message();   //进入异步接收
  }
 
private:
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
 
  void Callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        int len = msg->position.size();
        for (int j = 0; j < len && j < 5; j++) {
            pos[j] = msg->position[j];
        }
        if (len >= 5) {
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "pos,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4]);
            for (int i = 0; i < 5; i++) {
                joint[i] = static_cast<int>(pos[i] * 10000);
            }
            writeSpeed(joint, testSend3); // 调用自定义的串口发送函数
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Data was transmitted.\n");
        }
    }
 
  void writeSpeed(int *p, unsigned char ctrlFlag)
    {
        unsigned char buf[20] = {0};
        int i, length = 0;
    
        // 处理输入的速度数据并填充 angle 数组
        for (int i = 0; i < 7; i++)
        {
            angle[i].d = p[i];
        }
    
        // 填充消息头
        for (i = 0; i < 2; i++)
            buf[i] = header[i];
    
        length = 11;  // 定义消息长度
        buf[2] = length;  // 填充长度字段
    
        // 填充数据部分
        for (i = 0; i < 2; i++)
        {
            buf[i + 3] = angle[0].data[i];
            buf[i + 5] = angle[1].data[i];
            buf[i + 7] = angle[2].data[i];
            buf[i + 9] = angle[3].data[i];
            buf[i + 11] = angle[4].data[i];
        }
    
        // 填充控制标志
        buf[3 + length - 1] = ctrlFlag;
    
        // 计算并填充校验和
        buf[3 + length] = getCrc8(buf, 3 + length);
    
        // 填充消息尾部
        buf[3 + length + 1] = ender[0];
        buf[3 + length + 2] = ender[1];
    
        // 将 buf 数组转换为 std::vector<uint8_t>
        std::vector<uint8_t> data_to_send(buf, buf + 20);

        // 打印发送的数据，不换行
        RCLCPP_INFO(this->get_logger(), "Data to send: ");
        std::ostringstream data_stream;
        for (size_t j = 0; j < data_to_send.size(); ++j)
        {
            data_stream << "0x" << std::hex << std::uppercase << (int)data_to_send[j] << " ";  // 拼接每个字节的数据
        }
        RCLCPP_INFO(this->get_logger(), "%s", data_stream.str().c_str());  // 在同一行打印所有数据

        try
        {
            // 使用 serial_driver 发送数据
            size_t bytes_transmit_size = serial_driver_->port()->send(data_to_send); // 发送数据
            RCLCPP_INFO(this->get_logger(), "Sent data: %ld bytes!", bytes_transmit_size);  // 打印发送的字节数
        }
        catch (const std::exception &ex)
        {
            RCLCPP_ERROR(this->get_logger(), "Error sending data: %s", ex.what());  // 错误处理
        }
    }
  
  

  unsigned char getCrc8(unsigned char *ptr, unsigned short len)
    {
        unsigned char crc;
        unsigned char i;
        crc = 0;
        while (len--)
        {
            crc ^= *ptr++;
            for (i = 0; i < 8; i++)
            {
                if (crc & 0x01)
                    crc = (crc >> 1) ^ 0x8C;
                else
                    crc >>= 1;
            }
        }
        return crc;
    }
 
  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  std::shared_ptr<drivers::common::IoContext> io_context_;
  rclcpp::TimerBase::SharedPtr transmit_timer_;
  std::vector<uint8_t> transmit_data_buffer = std::vector<uint8_t>(1024); // 发送缓冲区
  std::vector<uint8_t> receive_data_buffer = std::vector<uint8_t>(1024);  // 接收缓冲区
};
 
int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
 
  auto node = std::make_shared<Serial_Node>();
 
  rclcpp::spin(node);
 
  rclcpp::shutdown();
  return 0;
}

